class EP_FP{
	public:
		EP_FP(mat Sig, mat b)
		{
			_Sig=Sig;
			_b=b.t();
			_p=_Sig.n_cols;
			d=new boost::math::normal_distribution<>(0,1); 
			rt=new rtnormNC();
			
			boost::random::uniform_01<> U;
			_runif=new RandomG::Random<boost::random::uniform_01<> >(U);
		}
		void Approx(void);
		mat operator()(int j, mat x,int k);


	private:
		mat _Sig;
		mat _b;
	       	mat _L;
		double sig;
		mat _mu_t;
		rtnormNC *rt;
		vector<mat> hSig;
	       	vector<mat> hm;
		int _p;	
		boost::math::normal_distribution<> *d;
		RandomG::Random<boost::random::uniform_01<> > *_runif;
};

void EP_FP::Approx(void){
	
	
	mat m;
	mat Sig(_p,_p);
	Sig.fill(1);
	Sig=diagmat(Sig); 
	mat t(_p,1);
	t.fill(0);
	m=t;

	hSig.push_back(Sig);// put first approx (Sig m for last iter)
	mat L=chol(_Sig);
	L=L.t();
	_L=L;
	hm.push_back(m);
	for(int i=_p-1;i>=0;i--)
	{
		mat G=L(i,span(0,i));
		double norm2=sqrt(as_scalar(G*Sig*G.t()));
		mat dlZm=(1/(1-cdf(*d,as_scalar((_b(i,0)-dot(G,m))/norm2)))*pdf(*d,as_scalar((_b(i,0)-dot(G,m))/norm2)))*G/norm2;	
		mat p2=0.5*(G.t()*G)*(_b(i,0)-dot(G,m))/pow(norm2,3);
		mat dlZS=(1/(1-cdf(*d,as_scalar((_b(i,0)-dot(G,m))/norm2)))*pdf(*d,as_scalar((_b(i,0)-dot(G,m))/norm2)))*p2;
		m=m+Sig*dlZm.t();	
		Sig=Sig-Sig*(dlZm.t()*dlZm-2*dlZS)*Sig;
		if(i>0)
		{
			hSig.push_back(Sig(span(0,i-1),span(0,i-1)));
			hm.push_back(m(span(0,i-1),0));
			m=m(span(0,i-1),0);	
			Sig=Sig(span(0,i-1),span(0,i-1));
			//cout << "Sig " << Sig;
			//cout << "m " << m;
		}
		
	}

}

mat EP_FP::operator()(int j, mat x,int k){
	mat m=hm[_p-j-1];	
	mat S=hSig[_p-j-1];
	double mu, sig;
	if(j>0)
	{
		//cout << x;
		if(k==0)
		{
			sig=sqrt(S(j,j)-as_scalar(S(j,span(0,j-1))*inv(S(span(0,j-1),span(0,j-1)))*S(j,span(0,j-1)).t()));
			_mu_t=S(j,span(0,j-1))*inv(S(span(0,j-1),span(0,j-1)));
		}
			mu=m(j,0)+as_scalar(_mu_t*(x(0,span(0,j-1)).t()-m(span(0,j-1),0)));
	}else{
		mu=m(j,0);
		sig=sqrt(S(j,j));
	}
	double sum=_b(j,0);
	for(int i=0;i<j;i++) sum-=_L(j,i)*x(0,i);
	sum=sum/(_L(j,j));
	//cout << "sum " << sum;
	//truncated gauusian greater than sum
	double u = (*_runif)(); 
	double arg=u+(1.-u)*cdf(*d,(sum-mu)/sig);
	if(arg > .999999999) arg=.999999999;
	if(arg < .0000000001) arg=.0000000001;
	double z = quantile(*d,arg)*sig+mu;
	double w=(1-cdf(*d,(sum-mu)/sig))*pdf(*d,z)*sig/(pdf(*d,(z-mu)/sig));
	mat res(2,1);
	res(0,0)=z;
	res(1,0)=w;	
	return res;
}
